#include "SCServo.h"
#include <unistd.h>
#include <stdio.h>


int main()
{
  Uart_Init();

  printf("cmd Ping:-------------------------------\n");
  int ret = -1;
  for (int id = 1; id <= 3; id++) {
    ret = Ping(id);
    if(ret < 0) {
      printf("Ping Servo ID %d fail, retrun %d!\n", id, ret);
    } else {
      printf("Ping Servo ID %d success, return %d!\n", id, ret);
    }
  }
  sleep(1);

  printf("cmd SetMode:----------------------------------\n");
  int ack = -1;
  for (int id = 1; id <= 3; id++) {
    ack = SetMode(id, STS_MODE_MULTI_TURN); // 设置一次后，模式掉电可保持
    if (ack < 0) {
      printf("Set Mode Servo ID %d fail, return %d!\n", id, ack);
    } else {
      printf("Set Mode Servo ID %d success, return %d!\n", id, ack);
    }
  }
  sleep(1);

  int aim_deg[5] = {0, 180, 225, 0, 20};
  printf("cmd WritePos:----------------------------\n");
  for (int id = 1; id <= 3; id++) {
    ack = WritePosEx(id, ANGLE_TO_POS(aim_deg[id]), 0, 10);
    if (ack < 0) {
      printf("write ID %d pos error,ack: %d\n", id, ack);
    } else {
      printf("write ID %d pos success,ack: %d\n", id, ack);
    }
  }
  sleep(3);

  // 解扭矩锁
  for (int id = 1; id <= 3; id++) {
    ack = EnableTorque(id, 0);
    if (ack < 0) {
      printf("id %d, EnableTorque() is error\n", id);
    } else {
      printf("id %d, EnableTorque() is success\n", id);
    }
    sleep(1);
  }
  
  
  int Pos = -1;
  int array_pos[4] = {0};
  for(;;) {
    for (int id = 1; id <= 3; id++) {
      printf("cmd ReadPos %d :----------------------------\n", id);
      Pos = ReadPos(id);
      if(Pos < 0) {
        printf("read Servo ID %d pos error, return %d\n", id, Pos);
        continue;
      } else {
        array_pos[id] = Pos;
        // printf("read Servo ID %d pos success, Pos:%d,deg:%d\n", id, Pos, POS_TO_ANGLE(Pos));
      }
    }
    printf("read-> Pos1:%d,Pos2:%d,Pos3:%d\n",
      array_pos[1],
      array_pos[2],
      array_pos[3]);
    printf("       deg1:%d,deg2:%d,deg3:%d\n",
      POS_TO_ANGLE(array_pos[1]),
      POS_TO_ANGLE(array_pos[2]),
      POS_TO_ANGLE(array_pos[3]));
    // usleep(10*1000); // 10ms
  }

   // int ack = -1;
  // for (;;) {
  //   // 默认舵机模式
  //   ack = WritePosEx(1, 3000, 0, 50);
  //   printf("1.ack: %d\n", ack);
  //   sleep(2);
  //   ack = WritePosEx(1, 1000, 0, 50);
  //   printf("2.ack: %d\n", ack);
  //   sleep(2);
  //   ack = WritePosEx(2, 3000, 0, 50);
  //   printf("3.ack: %d\n", ack);
  //   sleep(2);
  //   ack = WritePosEx(2, 1000, 0, 50);
  //   printf("4.ack: %d\n", ack);
  //   sleep(2);
  //   ack = WritePosEx(3, 3000, 0, 50);
  //   printf("5.ack: %d\n", ack);
  //   sleep(2);
  //   ack = WritePosEx(3, 1000, 0, 50);
  //   printf("6.ack: %d\n", ack);
  //   sleep(2);
  // }

  Uart_DeInit();
  
  return 0;

}
